using BepuPhysics.Collidables;
using BepuUtilities;
using System;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.CollisionDetection.CollisionTasks
{
    public struct CapsuleCylinderTester : IPairTester<CapsuleWide, CylinderWide, Convex2ContactManifoldWide>
    {
        public int BatchSize => 32;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void Bounce(in Vector3Wide lineOrigin, in Vector3Wide lineDirection, in Vector<float> t, in CylinderWide b, in Vector<float> radiusSquared, out Vector3Wide p, out Vector3Wide clamped)
        {
            // 将胶囊线上的点夹紧到圆柱体的边界,然后将夹紧的结果投影回线上。
            p.X = lineDirection.X * t + lineOrigin.X;
            p.Y = lineDirection.Y * t + lineOrigin.Y;
            p.Z = lineDirection.Z * t + lineOrigin.Z;
            var horizontalDistanceSquared = p.X * p.X + p.Z * p.Z;
            var needHorizontalClamp = Vector.GreaterThan(horizontalDistanceSquared, radiusSquared);
            var clampScale = b.Radius / Vector.SquareRoot(horizontalDistanceSquared);
            clamped.X = Vector.ConditionalSelect(needHorizontalClamp, clampScale * p.X, p.X);
            clamped.Y = Vector.Max(-b.HalfLength, Vector.Min(b.HalfLength, p.Y));
            clamped.Z = Vector.ConditionalSelect(needHorizontalClamp, clampScale * p.Z, p.Z);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void GetClosestPointBetweenLineSegmentAndCylinder(in Vector3Wide lineOrigin, in Vector3Wide lineDirection, in Vector<float> halfLength, in CylinderWide b,
            in Vector<int> inactiveLanes, out Vector<float> t, out Vector3Wide offsetFromCylinderToLineSegment)
        {
            var min = -halfLength;
            var max = halfLength;
            t = Vector<float>.Zero;
            var radiusSquared = b.Radius * b.Radius;
            Vector3Wide.Dot(lineDirection, lineOrigin, out var originDot);
            var epsilon = halfLength * 1e-7f;
            var laneDeactivated = inactiveLanes;
            for (int i = 0; i < 12; ++i)
            {
                Bounce(lineOrigin, lineDirection, t, b, radiusSquared, out _, out var clamped);
                Vector3Wide.Dot(clamped, lineDirection, out var conservativeNewT);
                conservativeNewT = Vector.Max(min, Vector.Min(max, conservativeNewT - originDot));
                var change = conservativeNewT - t;
                // 检查停用的车道,看看我们能不能早点离开。
                var laneShouldDeactivate = Vector.LessThan(Vector.Abs(change), epsilon);
                laneDeactivated = Vector.BitwiseOr(laneDeactivated, laneShouldDeactivate);
                if (Vector.LessThanAll(laneDeactivated, Vector<int>.Zero))
                {
                    // 所有车道都修好了;早点出发。
                    break;
                }

                // 这一反弹的预测可以被认为是保守的进步。更改的标志告诉我们前进的方向;我们可以用它来更新边界。
                var movedUp = Vector.GreaterThan(change, Vector<float>.Zero);
                min = Vector.ConditionalSelect(movedUp, conservativeNewT, min);
                max = Vector.ConditionalSelect(movedUp, max, conservativeNewT);

                // 将剩余的间隔一分为二。
                var newT = 0.5f * (min + max);

                // 停用的车道不应更新;如果迭代计数对捆绑包的行为敏感,则会创建对捆绑包顺序的依赖,并扼杀确定性。
                t = Vector.ConditionalSelect(laneDeactivated, t, newT);

            }
            Bounce(lineOrigin, lineDirection, t, b, radiusSquared, out var pointOnLine, out var clampedToCylinder);
            Vector3Wide.Subtract(pointOnLine, clampedToCylinder, out offsetFromCylinderToLineSegment);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void GetContactIntervalBetweenSegments(in Vector<float> aHalfLength, in Vector<float> bHalfLength, in Vector3Wide axisA, in Vector3Wide localNormal,
            in Vector<float> inverseHorizontalNormalLengthSquaredB, in Vector3Wide offsetB, out Vector<float> contactTMin, out Vector<float> contactTMax)
        {
            GetClosestPointsBetweenSegments(axisA, offsetB, aHalfLength, bHalfLength, out var ta, out var taMin, out var taMax, out var tb, out var tbMin, out var tbMax);

            // 在两个轴共面的情况下,我们接受整个间隔作为接触源。
            // 当轴线偏离共面时,可接受的间隔迅速缩小到零长度,以ta和Tb为中心。
            // 我们根据胶囊轴线与由侧面和接触法线定义的平面之间的夹角来评定共面程度：
            // sin(角度)=点(da,(Db X Normal)/||db x Normal||)
            // 最后,请注意,我们处理的是极小的角度,并且对于小角度sin(Angle)~=角度,
            // 而且淡入淡出的行为是完全任意的,所以我们可以直接使用平方角,而不用担心。
            // 角度^2~=点(da,(Db X Normal))^2/||db x Normal|^2
            // 请注意,由于db为(0,1,0),因此db x Normal仅为(Normal.Z,-Normal.X)。
            var dot = (axisA.X * localNormal.Z - axisA.Z * localNormal.X);
            var squaredAngle = dot * dot * inverseHorizontalNormalLengthSquaredB;

            // 将平方角度转换为LERP参数。对于从0到LowerThreshold的平方角度,我们应该使用完整的间隔(1)。从lowerThreshold到upperThreshold,将LERP设置为0。
            const float lowerThresholdAngle = 0.02f;
            const float upperThresholdAngle = 0.15f;
            const float lowerThreshold = lowerThresholdAngle * lowerThresholdAngle;
            const float upperThreshold = upperThresholdAngle * upperThresholdAngle;
            var intervalWeight = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, (new Vector<float>(upperThreshold) - squaredAngle) * new Vector<float>(1f / (upperThreshold - lowerThreshold))));
            // 如果线段相交,即使它们是共面的,理想情况下我们也会坚持使用单个点。会很容易,
            // 但我们不费心,因为这是一个非常奇怪和非常暂时的角落案件。真的不值得处理。
            var weightedTb = tb - tb * intervalWeight;
            contactTMin = intervalWeight * tbMin + weightedTb;
            contactTMax = intervalWeight * tbMax + weightedTb;
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void GetClosestPointsBetweenSegments(in Vector3Wide da, in Vector3Wide localOffsetB, in Vector<float> aHalfLength, in Vector<float> bHalfLength,
            out Vector<float> ta, out Vector<float> taMin, out Vector<float> taMax, out Vector<float> tb, out Vector<float> tbMin, out Vector<float> tbMax)
        {
            // 这类似于胶囊对执行,但是我们利用了这样一个事实,即我们在柱面的本地空间中工作,其中db=(0,1,0)。

            // 计算两条直线段之间最近的点。一开始就不要夹紧。
            // 我们希望最小化距离=||(a+da*ta)-(b+db*tb)||。
            // 计算关于ta的导数并进行一些代数运算(考虑||da||=||db||==1)来求解ta的结果：
            // Ta=(da*(b-a)+(db*(a-b))*(da*db))/(1-((da*db)*(da*db)
            Vector3Wide.Dot(da, localOffsetB, out var daOffsetB);
            var dbOffsetB = localOffsetB.Y;
            var dadb = da.Y;
            // 请注意,当轴线平行时,电势除以零。任意钳位;接近零值会产生极值,而这些极值被钳位到合理的结果。
            ta = (daOffsetB - dbOffsetB * dadb) / Vector.Max(new Vector<float>(1e-15f), Vector<float>.One - dadb * dadb);
            // tb=ta*(da*db)-db*(b-a)
            tb = ta * dadb - dbOffsetB;

            // 我们不能简单地将ta和tb值夹在胶囊线段上。取而代之的是,将每条线段投影到另一条线段上,夹紧目标的间隔。
            // 该新的钳制投影间隔是该直线段上的有效解空间。我们可以用这个区间钳制t值,得到正确的有界解。
            // 预计间隔为：
            // B到A：+-BHalfLength*(da*db)+da*offsetB
            // A到B：+-AHalfLength*(da*db)-db*offsetB
            var absdadb = Vector.Abs(dadb);
            var bOntoAOffset = bHalfLength * absdadb;
            var aOntoBOffset = aHalfLength * absdadb;
            taMin = Vector.Max(-aHalfLength, Vector.Min(aHalfLength, daOffsetB - bOntoAOffset));
            taMax = Vector.Min(aHalfLength, Vector.Max(-aHalfLength, daOffsetB + bOntoAOffset));
            tbMin = Vector.Max(-bHalfLength, Vector.Min(bHalfLength, -aOntoBOffset - dbOffsetB));
            tbMax = Vector.Min(bHalfLength, Vector.Max(-bHalfLength, aOntoBOffset - dbOffsetB));
            ta = Vector.Min(Vector.Max(ta, taMin), taMax);
            tb = Vector.Min(Vector.Max(tb, tbMin), tbMax);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold)
        {
            // 潜在的正常生成器：
            // 胶囊端点与气缸盖平面：
            // 胶囊端点与气缸盖边缘：仅适用于浅碰撞期间;如果胶囊线段与圆柱体相交,则端点的最快出路是笔直向上或侧向,而不是对角线。
            // 胶囊端点与圆柱面：
            // 胶囊边缘与气缸盖平面：与端点平面冗余;胶囊段不能比其任何一个端点更接近任何平面。
            // 胶囊边缘VS气缸盖边缘：这个太恶心了！
            // 胶囊边缘与气瓶侧面：如果覆盖了其他情况,这只是测线之间最接近的点(胶囊内线与气瓶内线)。

            // 胶囊边缘对气缸盖边缘是令人讨厌的。它涉及到寻找四次多项式的根,我们将避免这样做。

            // 而应使用寻求圆柱体和线段之间最小距离的数值求解器。
            // 这将捕获每个仅浅的法线候选对象,因此不需要执行胶囊端点对圆柱体封口边的操作。
            // 这有点作弊-解算器不能处理线段-圆柱体相交的情况,所以我们在深度碰撞中错过了一些边法线。
            // 因此,目前的正常生成器是：
            // 胶囊段与圆柱体：数值求解
            // 胶囊端点与气缸盖平面：仅当任何车道未通过浅距离测试时才需要
            // 胶囊段与圆柱侧：结合端点和边缘测试;仅当任何车道未通过浅距离测试时才需要
            Matrix3x3Wide.CreateFromQuaternion(orientationA, out var worldRA);
            Matrix3x3Wide.CreateFromQuaternion(orientationB, out var worldRB);
            // 在圆柱体的局部空间工作。
            Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(worldRA, worldRB, out var rA);
            ref var capsuleAxis = ref rA.Y;
            Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, worldRB, out var localOffsetB);
            Vector3Wide.Negate(localOffsetB, out var localOffsetA);

            ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes);
            GetClosestPointBetweenLineSegmentAndCylinder(localOffsetA, capsuleAxis, a.HalfLength, b, inactiveLanes, out var t, out var localNormal);
            Vector3Wide.LengthSquared(localNormal, out var distanceFromCylinderToLineSegmentSquared);
            var internalLineSegmentIntersected = Vector.LessThan(distanceFromCylinderToLineSegmentSquared, new Vector<float>(1e-12f));
            var distanceFromCylinderToLineSegment = Vector.SquareRoot(distanceFromCylinderToLineSegmentSquared);
            // 除以零受深度选择的保护-如果距离为零,则深度设置为无穷大,并且不会选择该法线。
            Vector3Wide.Scale(localNormal, Vector<float>.One / distanceFromCylinderToLineSegment, out localNormal);
            var depth = Vector.ConditionalSelect(internalLineSegmentIntersected, new Vector<float>(float.MaxValue), -distanceFromCylinderToLineSegment);

            if (Vector.LessThanAny(Vector.AndNot(internalLineSegmentIntersected, inactiveLanes), Vector<int>.Zero))
            {
                // 至少有一条车道相交很深,所以我们需要检查其他可能的法线。
                var endpointVsCapDepth = b.HalfLength + Vector.Abs(capsuleAxis.Y * a.HalfLength) - Vector.Abs(localOffsetA.Y);
                var useEndpointCapDepth = Vector.BitwiseAnd(internalLineSegmentIntersected, Vector.LessThan(endpointVsCapDepth, depth));
                depth = Vector.ConditionalSelect(useEndpointCapDepth, endpointVsCapDepth, depth);
                localNormal.X = Vector.ConditionalSelect(useEndpointCapDepth, Vector<float>.Zero, localNormal.X);
                // 法线校准为从B指向A。
                localNormal.Y = Vector.ConditionalSelect(useEndpointCapDepth, Vector.ConditionalSelect(Vector.GreaterThan(localOffsetA.Y, Vector<float>.Zero), Vector<float>.One, new Vector<float>(-1f)), localNormal.Y);
                localNormal.Z = Vector.ConditionalSelect(useEndpointCapDepth, Vector<float>.Zero, localNormal.Z);

                GetClosestPointsBetweenSegments(capsuleAxis, localOffsetB, a.HalfLength, b.HalfLength, out var ta, out _, out _, out var tb, out _, out _);

                // 偏移量=da*ta-(db*tb+offsetB)
                Vector3Wide.Scale(capsuleAxis, ta, out var closestA);
                Vector3Wide.Subtract(closestA, localOffsetB, out var offset);
                offset.Y -= tb;

                Vector3Wide.Length(offset, out var distance);
                var inverseDistance = Vector<float>.One / distance;
                Vector3Wide.Scale(offset, inverseDistance, out var internalEdgeNormal);
                var useFallback = Vector.LessThan(distance, new Vector<float>(1e-7f));
                internalEdgeNormal.X = Vector.ConditionalSelect(useFallback, Vector<float>.One, internalEdgeNormal.X);
                internalEdgeNormal.Y = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, internalEdgeNormal.Y);
                internalEdgeNormal.Z = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, internalEdgeNormal.Z);

                // 计算沿内边法线的深度。
                Vector3Wide.Dot(localOffsetA, internalEdgeNormal, out var centerSeparationAlongNormal);
                var cylinderContribution = Vector.Abs(b.HalfLength * internalEdgeNormal.Y) + b.Radius * Vector.SquareRoot(Vector.Max(Vector<float>.Zero, Vector<float>.One - internalEdgeNormal.Y * internalEdgeNormal.Y));
                Vector3Wide.Dot(capsuleAxis, internalEdgeNormal, out var capsuleAxisDotNormal);
                var capsuleContribution = Vector.Abs(capsuleAxisDotNormal) * a.HalfLength;
                var internalEdgeDepth = cylinderContribution + capsuleContribution - centerSeparationAlongNormal;

                var useInternalEdgeDepth = Vector.BitwiseAnd(internalLineSegmentIntersected, Vector.LessThan(internalEdgeDepth, depth));
                depth = Vector.ConditionalSelect(useInternalEdgeDepth, internalEdgeDepth, depth);
                Vector3Wide.ConditionalSelect(useInternalEdgeDepth, internalEdgeNormal, localNormal, out localNormal);
            }
            // 所有上述因素都排除了对太空舱半径的任何考虑。现在就包括在内。
            depth += a.Radius;
            inactiveLanes = Vector.BitwiseOr(Vector.LessThan(depth, -speculativeMargin), inactiveLanes);
            if (Vector.LessThanAll(inactiveLanes, Vector<int>.Zero))
            {
                // 所有车道都有一个深度,由于投机性边际的原因,该深度不能创建任何接触。我们可以早点出发。
                // 这是确定性安全的;即使继续执行,也不会创建任何触点,并且所有通道的歧管都是等效的。
                manifold = default;
                return;
            }
            // 我们现在有了深度和碰撞法线。使用它来标识每个形状上的代表性特征。
            // 请注意,我们排除边作为代表;仅使用带面积的特征,以便接触生成可以产生更好的推测接触。
            // 潜在功能对包括：
            // 胶囊段与圆柱体侧面
            // 胶囊段与气缸盖
            // 管段侧情况的处理方式与胶囊-胶囊相同-通过将管段投影到圆柱体管段上来创建间隔,然后根据非共面缩小间隔。
            // 分段封口也很容易;将分段向下投影到封口平面上。将其与封口圆对齐(求解二次曲线)。

            var useCapContacts = Vector.GreaterThan(Vector.Abs(localNormal.Y), new Vector<float>(0.70710678118f));

            // 首先,假设无盖触点。
            // 将问题表述为分段测试。
            // 对于任何需要侧接触的车道,我们知道基于上述条件,投影法线长度将不为零。
            var inverseHorizontalNormalLengthSquared = Vector<float>.One / (localNormal.X * localNormal.X + localNormal.Z * localNormal.Z);
            var scale = b.Radius * Vector.SquareRoot(inverseHorizontalNormalLengthSquared);
            var cylinderSegmentOffsetX = localNormal.X * scale;
            var cylinderSegmentOffsetZ = localNormal.Z * scale;
            Vector3Wide aToSideSegmentCenter;
            aToSideSegmentCenter.X = localOffsetB.X + cylinderSegmentOffsetX;
            aToSideSegmentCenter.Y = localOffsetB.Y;
            aToSideSegmentCenter.Z = localOffsetB.Z + cylinderSegmentOffsetZ;
            GetContactIntervalBetweenSegments(a.HalfLength, b.HalfLength, capsuleAxis, localNormal, inverseHorizontalNormalLengthSquared, aToSideSegmentCenter, out var contactTMin, out var contactTMax);

            Vector3Wide contact0, contact1;
            contact0.X = cylinderSegmentOffsetX;
            contact0.Y = contactTMin;
            contact0.Z = cylinderSegmentOffsetZ;
            contact1.X = cylinderSegmentOffsetX;
            contact1.Y = contactTMax;
            contact1.Z = cylinderSegmentOffsetZ;

            var contactCount = Vector.ConditionalSelect(Vector.LessThan(Vector.Abs(contactTMax - contactTMin), b.HalfLength * new Vector<float>(1e-5f)), Vector<int>.One, new Vector<int>(2));

            if (Vector.LessThanAny(Vector.AndNot(useCapContacts, inactiveLanes), Vector<int>.Zero))
            {
                // 至少有一条车道需要端头触点。
                // 重要说明：为了获得最高质量,所有裁剪都在*法线平面上进行。因此,段帽不只是将y分量设置为零(沿B的Y轴投影)。
                // 相反,端点沿法线投射到封口平面。
                // t=点(胶囊原点+-胶囊方向*a.半长度-(0,Normal.Y>0？b.HalfLength：a.HalfLength,0),圆柱体Y)/点(法线,圆柱体Y)
                // t=(封装原点.Y+-封装方向.Y*a.HalfLength-(Normal.Y>0？b.HalfLength：a.HalfLength))/Normal.Y
                // 请注意,仅当Normal.Y控制水平方向时,才会选择封口作为代表,因此不需要测试是否除以零。
                var capHeight = Vector.ConditionalSelect(Vector.GreaterThan(localNormal.Y, Vector<float>.Zero), b.HalfLength, -b.HalfLength);
                var inverseNormalY = Vector<float>.One / localNormal.Y;
                Vector3Wide.Scale(capsuleAxis, a.HalfLength, out var endpointOffset);
                Vector3Wide positive, negative;
                positive.X = localOffsetA.X + endpointOffset.X;
                positive.Y = localOffsetA.Y + endpointOffset.Y - capHeight;
                positive.Z = localOffsetA.Z + endpointOffset.Z;
                negative.X = localOffsetA.X - endpointOffset.X;
                negative.Y = localOffsetA.Y - endpointOffset.Y - capHeight;
                negative.Z = localOffsetA.Z - endpointOffset.Z;
                var centerOffsetAlongY = localOffsetA.Y - capHeight;
                var endpointOffsetAlongY = capsuleAxis.Y * a.HalfLength;
                var tNegative = negative.Y * inverseNormalY;
                var tPositive = positive.Y * inverseNormalY;
                Vector2Wide projectedPositive, projectedNegative;
                projectedNegative.X = negative.X - localNormal.X * tNegative;
                projectedNegative.Y = negative.Z - localNormal.Z * tNegative;
                projectedPositive.X = positive.X - localNormal.X * tPositive;
                projectedPositive.Y = positive.Z - localNormal.Z * tPositive;

                // 使线段(projectedNegative、projectedPositive)与半径为b.Radius位于(0,0)的圆相交。
                Vector2Wide.Subtract(projectedPositive, projectedNegative, out var projectedOffset);
                // |a+ab*t||=半径
                // 点(a+ab*t,a+ab*t)=半径*半径
                // 点(a,a)-半径*半径+t*2*点(a,ab)+t^2*点(ab,ab)=0
                Vector2Wide.Dot(projectedNegative, projectedNegative, out var coefficientC);
                coefficientC -= b.Radius * b.Radius;
                Vector2Wide.Dot(projectedNegative, projectedOffset, out var coefficientB);
                Vector2Wide.Dot(projectedOffset, projectedOffset, out var coefficientA);
                var inverseA = Vector<float>.One / coefficientA;
                var tOffset = Vector.SquareRoot(Vector.Max(Vector<float>.Zero, coefficientB * coefficientB - coefficientA * coefficientC)) * inverseA;
                var tBase = -coefficientB * inverseA;
                var tMin = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, tBase - tOffset));
                var tMax = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, tBase + tOffset));
                // 如果投影长度为零,只需将这两个点视为位于同一位置(在tNegative)。
                var useFallback = Vector.LessThan(Vector.Abs(coefficientA), new Vector<float>(1e-12f));
                tMin = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, tMin);
                tMax = Vector.ConditionalSelect(useFallback, Vector<float>.Zero, tMax);
                Vector3Wide capContact0, capContact1;
                capContact0.X = tMin * projectedOffset.X + projectedNegative.X;
                capContact0.Y = capHeight;
                capContact0.Z = tMin * projectedOffset.Y + projectedNegative.Y;
                capContact1.X = tMax * projectedOffset.X + projectedNegative.X;
                capContact1.Y = capHeight;
                capContact1.Z = tMax * projectedOffset.Y + projectedNegative.Y;
                // Fixed epsilon-t值缩放偏移,该偏移通常与对象大小成比例。
                var capContactCount = Vector.ConditionalSelect(Vector.GreaterThan(tMax - tMin, new Vector<float>(1e-5f)), new Vector<int>(2), Vector<int>.One);
                contactCount = Vector.ConditionalSelect(useCapContacts, capContactCount, contactCount);
                Vector3Wide.ConditionalSelect(useCapContacts, capContact0, contact0, out contact0);
                Vector3Wide.ConditionalSelect(useCapContacts, capContact1, contact1, out contact1);
            }
            // 虽然我们已经计算了全局深度,但每个接触都有自己的深度。
            // 将触点沿垂直于A的"面"的触点投影到B上。
            // A是一个胶囊,但是我们可以把它看作有一个faceNormalA=(LocalNormal X CapiteAxis)x CapiteAxis。
            // 全部计算如下：
            // t=点(触点+localOffsetB,faceNormalA)/点(faceNormalA,localNormal)
            // 深度=点(t*localNormal,localNormal)=t
            Vector3Wide.CrossWithoutOverlap(localNormal, capsuleAxis, out var capsuleTangent);
            Vector3Wide.CrossWithoutOverlap(capsuleTangent, capsuleAxis, out var faceNormalA);
            Vector3Wide.Dot(faceNormalA, localNormal, out var faceNormalADotLocalNormal);
            // 不需要对FaceNormalA执行任何校准;它同时出现在分子和分母中,因此符号和大小相互抵消。
            var inverseFaceNormalADotLocalNormal = Vector<float>.One / faceNormalADotLocalNormal;
            Vector3Wide.Scale(faceNormalA, faceNormalADotLocalNormal, out var scaledFaceNormalA);
            Vector3Wide.Add(localOffsetB, contact0, out var offset0);
            Vector3Wide.Add(localOffsetB, contact1, out var offset1);
            Vector3Wide.Dot(offset0, faceNormalA, out var t0);
            Vector3Wide.Dot(offset1, faceNormalA, out var t1);
            t0 *= inverseFaceNormalADotLocalNormal;
            t1 *= inverseFaceNormalADotLocalNormal;
            manifold.Depth0 = a.Radius + t0;
            manifold.Depth1 = a.Radius + t1;

            // 如果胶囊轴线与法线平行,则触点塌陷到一点,我们可以使用最初计算的深度。
            // 在这种情况下,两个接触位置无论如何都应该非常接近。
            var collapse = Vector.LessThan(Vector.Abs(faceNormalADotLocalNormal), new Vector<float>(1e-14f));
            manifold.Depth0 = Vector.ConditionalSelect(collapse, depth, manifold.Depth0);
            var negativeMargin = -speculativeMargin;
            manifold.Contact0Exists = Vector.GreaterThan(manifold.Depth0, negativeMargin);
            manifold.Contact1Exists = Vector.BitwiseAnd(Vector.AndNot(Vector.Equals(contactCount, new Vector<int>(2)), collapse), Vector.GreaterThan(manifold.Depth1, negativeMargin));

            // 将触点推入世界空间。
            Matrix3x3Wide.TransformWithoutOverlap(localNormal, worldRB, out manifold.Normal);
            Matrix3x3Wide.TransformWithoutOverlap(contact0, worldRB, out manifold.OffsetA0);
            Matrix3x3Wide.TransformWithoutOverlap(contact1, worldRB, out manifold.OffsetA1);
            Vector3Wide.Add(manifold.OffsetA0, offsetB, out manifold.OffsetA0);
            Vector3Wide.Add(manifold.OffsetA1, offsetB, out manifold.OffsetA1);

            manifold.FeatureId0 = Vector<int>.Zero;
            manifold.FeatureId1 = Vector<int>.One;
        }


        public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold)
        {
            throw new NotImplementedException();
        }

        public void Test(ref CapsuleWide a, ref CylinderWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold)
        {
            throw new NotImplementedException();
        }
    }
}
